import numpy as np
import math

x0_xyz_global = np.array([1.0, 1.0, 1.0])
r = math.sqrt(x0_xyz_global[0] * x0_xyz_global[0] + x0_xyz_global[1] * x0_xyz_global[1])
phi = math.atan(x0_xyz_global[1] / x0_xyz_global[0])

x0_rphiz_global = np.array([r, phi, 1.0])

x1_xyz_local = np.array([1.0, 1.0, 1.0])


rotation_reverse_j = np.zeros((3,3), dtype=float)

rotation_reverse_j[0,0] = math.cos(phi)
rotation_reverse_j[0,1] = -math.sin(phi)
rotation_reverse_j[1,0] = math.sin(phi)
rotation_reverse_j[1,1] = math.cos(phi)

rotation_reverse_j[0,2] = 0.0
rotation_reverse_j[1,2] = 0.0
rotation_reverse_j[2,0] = 0.0
rotation_reverse_j[2,1] = 0.0
rotation_reverse_j[2,2] = 1.0

#x1 local xyz to global xyz
x0_x1_global = np.zeros(3)
x0_x1_global = np.dot(rotation_reverse_j, x1_xyz_local)

x1_xyz_global = x0_xyz_global + x0_x1_global

x1_rphiz_global = np.zeros(3)
x1_rphiz_global[0] = math.sqrt(x1_xyz_global[0] * x1_xyz_global[0] + x1_xyz_global[1] * x1_xyz_global[1])
x1_rphiz_global[1] = math.atan(x1_xyz_global[1] / x1_xyz_global[0])
x1_rphiz_global[2] = x1_xyz_global[2]

print(x0_xyz_global)
print(x0_rphiz_global)

print(x1_xyz_global)
print(x1_rphiz_global)

